#include "hero_integrated_vision/cameraparams_manager.hpp"
#include "hero_integrated_vision/hero_vision_node.hpp"

namespace rm_aim
{

// -------------------------- 构造函数 --------------------------
CameraParamsManager::CameraParamsManager(HeroVisionNode* node)
    : node_(node), logger_(node->get_logger())
{
    // 初始化默认控制指令
    is_scope_enabled_ = false;    // 默认不使用倍镜

    // 订阅两套内参
    subscribeCameraParams();

    RCLCPP_INFO(logger_, "CameraParamsManager initialized (default: use_scope=false)");
}

// -------------------------- 核心接口 --------------------------
aim_interfaces::msg::Armors CameraParamsManager::generateArmorsMsg(
    const std::vector<Armor>& armors,
    const std_msgs::msg::Header& img_header)
{
    // 1. 初始化消息头
    armors_msg_.header = img_header;
    armors_msg_.armors.clear();

    // 2. 根据use_scope选择内参（关键一步）
    const CameraParams& selected_params = selectCameraParams();

    // 3. 内参未初始化 → 返回空消息
    if (!selected_params.is_initialized) {
        RCLCPP_WARN(logger_, "Selected camera params not initialized!");
        return armors_msg_;
    }

    // 4.普通自瞄/前哨站 → 处理装甲板
    processArmors(armors, selected_params);

    return armors_msg_;
}

// -------------------------- 内部辅助函数 --------------------------
void CameraParamsManager::subscribeCameraParams()
{
    // 订阅第一套内参（非倍镜：/camera_info）
    general_cam_sub_ = node_->create_subscription<sensor_msgs::msg::CameraInfo>(
        "/camera_info", rclcpp::SensorDataQoS(),
        [this](const sensor_msgs::msg::CameraInfo::ConstSharedPtr msg) {
            general_params_.cam_info = std::make_shared<sensor_msgs::msg::CameraInfo>(*msg);
            general_params_.cam_center = cv::Point2f(msg->k[2], msg->k[5]);
            general_params_.pnp_solver = std::make_unique<PnPSolver>(msg->k, msg->d);
            general_params_.is_initialized = true;
            RCLCPP_INFO(logger_, "General camera params received (non-scope)");
            general_cam_sub_.reset();
        });

    // 订阅第二套内参（倍镜：/scope_camera_info）
    scope_cam_sub_ = node_->create_subscription<sensor_msgs::msg::CameraInfo>(
        "/scope_camera_info", rclcpp::SensorDataQoS(),
        [this](const sensor_msgs::msg::CameraInfo::ConstSharedPtr msg) {
            scope_params_.cam_info = std::make_shared<sensor_msgs::msg::CameraInfo>(*msg);
            scope_params_.cam_center = cv::Point2f(msg->k[2], msg->k[5]);
            scope_params_.pnp_solver = std::make_unique<PnPSolver>(msg->k, msg->d);
            scope_params_.is_initialized = true;
            RCLCPP_INFO(logger_, "Scope camera params received (scope)");
            scope_cam_sub_.reset();
        });
}

const CameraParamsManager::CameraParams& CameraParamsManager::selectCameraParams() const
{
    // 核心逻辑：use_scope=true→第二套（倍镜），false→第一套（非倍镜）
    if (is_scope_enabled_) {
        RCLCPP_DEBUG(logger_, "Use scope: select scope camera params");
        return scope_params_;
    } else {
        RCLCPP_DEBUG(logger_, "Not use scope: select general camera params");
        return general_params_;
    }
}

void CameraParamsManager::processArmors(
    const std::vector<Armor>& armors,
    const CameraParams& selected_params)
{
    aim_interfaces::msg::Armor armor_msg;
    const auto& pnp_solver = selected_params.pnp_solver;
    const auto& cam_center = selected_params.cam_center;
    cv::Mat camera_matrix_mat(3, 3, CV_64F, const_cast<double*>(selected_params.cam_info->k.data()));
    cv::Mat distort_coeffs_mat(1, 5, CV_64F, const_cast<double*>(selected_params.cam_info->d.data()));

    for (const auto& armor : armors) {
        // 计算装甲板位置
        cv::Mat rvec, tvec;// 旋转向量、平移向量
        bool pnp_success = pnp_solver->solvePnP(armor, rvec, tvec);
        if (!pnp_success) continue;
        
        // 填充装甲板消息
        armor_msg.type = ARMOR_TYPE_STR[static_cast<int>(armor.type)];
        armor_msg.number = armor.number;

        auto is_balance = (armor.type == ArmorType::LARGE) && 
                          (armor.number == "3" || armor.number == "4" || armor.number == "5");
        if (is_balance) {
        // 平衡装甲板：直接用原始rvec/tvec构建Pose
        // 填充位姿信息
        // 位置信息（从平移向量获取）
        armor_msg.pose.position.x = tvec.at<double>(0);
        armor_msg.pose.position.y = tvec.at<double>(1);
        armor_msg.pose.position.z = tvec.at<double>(2);

        //旋转信息（旋转向量→旋转矩阵→四元数）
        cv::Mat rotation_matrix;
        cv::Rodrigues(rvec, rotation_matrix);  // 旋转向量转矩阵
        tf2::Matrix3x3 tf2_rot_mat(
            rotation_matrix.at<double>(0,0), rotation_matrix.at<double>(0,1), rotation_matrix.at<double>(0,2),
            rotation_matrix.at<double>(1,0), rotation_matrix.at<double>(1,1), rotation_matrix.at<double>(1,2),
            rotation_matrix.at<double>(2,0), rotation_matrix.at<double>(2,1), rotation_matrix.at<double>(2,2)
        );
        tf2::Quaternion tf2_quat;
        tf2_rot_mat.getRotation(tf2_quat); // 矩阵转四元数
        armor_msg.pose.orientation = tf2::toMsg(tf2_quat);
        } else {
            // 非平衡装甲板：调用函数得到优化后的Pose
            armor_msg.pose = pnp_solver->optimize_yaw_direct(rvec, tvec, armor, camera_matrix_mat, distort_coeffs_mat);
        }

        // 图像中心距离
        armor_msg.distance_to_image_center = cv::norm(armor.center_point - cam_center);

        armors_msg_.armors.emplace_back(armor_msg);
    }
}

void CameraParamsManager::processGreenLights(
    const std::vector<cv::Rect2f>& green_lights,
    const CameraParams& selected_params)
{
    // aim_interfaces::msg::Armor armor_msg;
    // const auto& pnp_solver = selected_params.pnp_solver;
    // const auto& cam_center = selected_params.cam_center;
    // const auto& cam_info = selected_params.cam_info;

    // // 绿灯3D模板点（按实际尺寸调整）
    // std::vector<cv::Point3f> gl_object_points = {
    //     {-0.05f, -0.05f, 0.0f}, {-0.05f, 0.05f, 0.0f},
    //     {0.05f, 0.05f, 0.0f},   {0.05f, -0.05f, 0.0f}
    // };

    // for (const auto& gl_rect : green_lights) {
    //     auto gl_corners = getGreenLightCorners(gl_rect);
    //     if (gl_corners.size() != 4) continue;

    //     cv::Mat rvec, tvec;
    //     bool pnp_success = pnp_solver->solvePnP(
    //         gl_object_points, gl_corners,
    //         cam_info->k, cam_info->d);
    //     if (!pnp_success) continue;

    //     // 填充绿灯消息
    //     armor_msg.type = "green_light";
    //     armor_msg.number = "green";
    //     armor_msg.pose.position.x = tvec.at<double>(0);
    //     armor_msg.pose.position.y = tvec.at<double>(1);
    //     armor_msg.pose.position.z = tvec.at<double>(2);
    //     armor_msg.pose.orientation.w = 1.0;
    //     armor_msg.pose.orientation.x = armor_msg.pose.orientation.y = armor_msg.pose.orientation.z = 0.0;

    //     // 绿灯中心距离
    //     cv::Point2f gl_center(gl_rect.x + gl_rect.width/2, gl_rect.y + gl_rect.height/2);
    //     armor_msg.distance_to_image_center = cv::norm(gl_center - cam_center);

    //     armors_msg_.armors.emplace_back(armor_msg);
    // }
}

std::vector<cv::Point2f> CameraParamsManager::getGreenLightCorners(const cv::Rect2f& rect)
{
    std::vector<cv::Point2f> corners;
    corners.emplace_back(rect.tl());          // 左上
    corners.emplace_back(rect.br().x, rect.tl().y);  // 右上
    corners.emplace_back(rect.br());          // 右下
    corners.emplace_back(rect.tl().x, rect.br().y);  // 左下
    return corners;
}

}  // namespace rm_aim